#include "matrix_inv.h"
#include "kalman_2state.h"
#include <iostream>
#include <fstream>
#include <time.h>

int main()
{

	double dt = 0.5;
	boost::numeric::ublas::matrix<double> meas_noise(1,1);
	boost::numeric::ublas::matrix<double> accel_noise(1,1);
	meas_noise(0,0) = 40;
	accel_noise(0,0) = 2;

	//Measurement error covariance
	boost::numeric::ublas::matrix<double> R(1,1);
	R(0,0) = pow(meas_noise(0,0),2);


	
	//Random Number Generator
	typedef boost::minstd_rand base_generator_type;
	boost::minstd_rand generator(time(NULL));
	boost::uniform_real<> uni_dist(-1,1);
	boost::variate_generator<base_generator_type&, boost::uniform_real<> > uni(generator, uni_dist);


	//Process Noise Covariance
	boost::numeric::ublas::matrix<double> Q(2,2);
	Q(0,0) = pow(accel_noise(0,0),2)*pow(dt,4)/4;
	Q(0,1) = pow(accel_noise(0,0),2)*pow(dt,3)/2;
	Q(1,0) = pow(accel_noise(0,0),2)*pow(dt,3)/2;
	Q(1,1) = pow(accel_noise(0,0),2)*pow(dt,2);


	//Initial State
	boost::numeric::ublas::matrix<double> state(2,1);
	state(0,0) = 10;
	state(1,0) = 5;

	//Initial State Estimate
	boost::numeric::ublas::matrix<double> state_estimate(2,1);
	state_estimate = state;

	//Initial State Covariance
	boost::numeric::ublas::matrix<double> covariance(2,2);
	covariance = Q;

	kalman_2state k2s(R , Q , dt );

	//Observation Matrix
	boost::numeric::ublas::matrix<double> H(1,2);
	H(0,0) = 1;
	H(0,1) = 0;


	std::ofstream dataOut("data.csv",std::ios::app);
	dataOut<<"Time,True,Est"<<std::endl;

	for(int i = 0 ; i < 100 ; i++)
	{
	double randn = uni();

	//Measurement
	boost::numeric::ublas::matrix<double> Z(1,1);
	Z  = prod( H , state ) + meas_noise * randn;

	//Process Noise
	boost::numeric::ublas::matrix<double> ProcessNoise(2,1);
	ProcessNoise(0,0) = accel_noise(0,0)*randn*pow(dt,2)/2;
	ProcessNoise(1,0) = accel_noise(0,0)*randn*dt;


	std::cout<<"Iteration["<<i<<"]"<<" State         : "<<state(0,0)<<std::endl;
	std::cout<<"Iteration["<<i<<"]"<<" State Estimate: "<<state_estimate(0,0)<<std::endl;

	dataOut<<dt*i<<","<<state(0,0)<<","<<state_estimate(0,0)<<std::endl;
	k2s.update(state , state_estimate , covariance , Z , ProcessNoise );

	}
        
        //addition from the mac

}